/*****************************************************
** Author:
** Date: 2019-08-03
*
*****************************************************/

#include <stdio.h>
#include <string.h>
#include "pca9685.h"

#define log_dbg(_fmt_, ...)   \
            printf("[PCA9685][%s:%d][%s][DBG] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_inf(_fmt_, ...)   \
            printf("[PCA9685][%s:%d][%s][INF] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_wrn(_fmt_, ...)   \
            printf("[PCA9685][%s:%d][%s][WRN] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_err(_fmt_, ...)   \
            printf("[PCA9685][%s:%d][%s][ERR] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)

#define PCA9685_CLK			25000000
#define PCA9685_FULL_BIT 	0x10

#define PRESCALE_TRANSFER(clk, freq)	(clk / 4096 / freq - 1)


typedef struct Pca9685Obj {
	Pca9685CbStru cb;
} Pca9685ObjStru;

typedef struct {
	Pca9685ObjStru *pca9685;
	Pca9685ParamStru param;
	unsigned short channel;
} ServoObjStru;

typedef struct {
	Pca9685ObjStru *pca9685;
	Pca9685ParamStru param;
	unsigned short channelForward;
	unsigned short channelBackward;
	int runState;
} DcMotorObjStru;

static void *(*Pca9685Malloc)(unsigned int size);
static void (*Pca9685Free)(void *ptr);

/**
 * @brief
 * @param  None
 * @retval None
 */
void Pca9685RegisterMalloc(void *(*malloc)(unsigned int size), void (*free)(void *ptr))
{
	if (malloc == NULL || free == NULL) {
		return;
	}

	Pca9685Malloc = malloc;
	Pca9685Free = free;
}

static void Pca9685RegWrite(Pca9685ObjStru *ctx, unsigned char reg, unsigned char val)
{
	unsigned char data[2];
	int ret;

	data[0] = reg;
	data[1] = val;
	ret = ctx->cb.i2cWriteCb(data, sizeof(data));
	log_dbg("i2c wr %#x %#x ret %d", data[0], data[1], ret);
	if (ret != 0) {
		log_err("i2c write failed");
	}
}

static unsigned char Pca9685RegRead(Pca9685ObjStru *ctx, unsigned char reg)
{
	unsigned char data;
	ctx->cb.i2cWriteCb(&reg, sizeof(unsigned char));
	ctx->cb.i2cReadCb(&data, sizeof(unsigned char));
	return data;
}

static void Pca9685SetFreq(Pca9685ObjStru *ctx, int freq)
{
	unsigned char prescale = PRESCALE_TRANSFER(PCA9685_CLK, freq);
	unsigned char tmp, val;

	Pca9685RegWrite(ctx, PCA9685_REG_MODE1, 0x00);

	/* set prescale during sleep mode */
	tmp = Pca9685RegRead(ctx, PCA9685_REG_MODE1);
	val = (tmp & 0x7f) | PCA9685_MODE1_SLEEP_BIT;
	Pca9685RegWrite(ctx, PCA9685_REG_MODE1, val);
	Pca9685RegWrite(ctx, PCA9685_REG_PRE_SCALE, prescale);
	Pca9685RegWrite(ctx, PCA9685_REG_MODE1, tmp);

	ctx->cb.delayMsCb(5);

	/* restart */
	val = tmp | PCA9685_MODE1_RESTART_BIT;
	Pca9685RegWrite(ctx, PCA9685_REG_MODE1, val);

	/* set outputs are configured with a totem pole structure */
	Pca9685RegWrite(ctx, PCA9685_REG_MODE2, 0x04);
}

Pca9685ObjHdl Pca9685Init(Pca9685CbStru *cfg, int freq)
{
	Pca9685ObjStru *ctx;

	ctx = (Pca9685ObjStru *)Pca9685Malloc(sizeof(Pca9685ObjStru));
	if (ctx == NULL) {
		return NULL;
	}
	memcpy(&ctx->cb, cfg, sizeof(Pca9685CbStru));
	Pca9685SetFreq(ctx, freq);
	return (Pca9685ObjHdl)ctx;
}

void Pca9685Deinit(Pca9685ObjHdl hdl)
{
	Pca9685ObjStru *ctx = (Pca9685ObjStru *)hdl;
	Pca9685RegWrite(ctx, PCA9685_REG_MODE2, 0x00);
	Pca9685Free(ctx);
}

/*
 *	cycle: _________|^^^^^^^^|______
 *		   |<- on ->|
 *		   |<----- off ----->|
 *		   |<-------- 4096 -------->|
 */
void Pca9685SetPwm(Pca9685ObjHdl hdl, unsigned short channel, unsigned short on, unsigned short off)
{
	Pca9685ObjStru *ctx = (Pca9685ObjStru *)hdl;
	Pca9685RegWrite(ctx, PCA9685_REG_LED0_ON_L  + 4 * channel, on & 0x00ff);
	Pca9685RegWrite(ctx, PCA9685_REG_LED0_ON_H  + 4 * channel, (on >> 8) & (~PCA9685_FULL_BIT));
	Pca9685RegWrite(ctx, PCA9685_REG_LED0_OFF_L + 4 * channel, off & 0x00ff);
	Pca9685RegWrite(ctx, PCA9685_REG_LED0_OFF_H + 4 * channel, (off >> 8) & (~PCA9685_FULL_BIT));
	log_dbg("channel %d on %d off %d", channel, on, off);
}

void Pca9685SetPwmFull(Pca9685ObjHdl hdl, unsigned short channel, bool full)
{
	Pca9685ObjStru *ctx = (Pca9685ObjStru *)hdl;
	unsigned char onHigh  = Pca9685RegRead(ctx, PCA9685_REG_LED0_ON_H  + 4 * channel);
	unsigned char offHigh = Pca9685RegRead(ctx, PCA9685_REG_LED0_OFF_H + 4 * channel);

	if (full) { /* full on */
		Pca9685RegWrite(ctx, PCA9685_REG_LED0_ON_H  + 4 * channel, onHigh | PCA9685_FULL_BIT);
		Pca9685RegWrite(ctx, PCA9685_REG_LED0_OFF_H + 4 * channel, offHigh & (~PCA9685_FULL_BIT));
	} else { /* full off */
		Pca9685RegWrite(ctx, PCA9685_REG_LED0_OFF_H + 4 * channel, offHigh | PCA9685_FULL_BIT);
		Pca9685RegWrite(ctx, PCA9685_REG_LED0_ON_H  + 4 * channel, onHigh & (~PCA9685_FULL_BIT));
	}
	log_dbg("channel %d full %d", channel, full);
}

ServoObjHdl ServoInit(Pca9685ObjHdl pca9685, Pca9685ParamStru *param, unsigned short channel)
{
	ServoObjStru *ctx;

	ctx = (ServoObjStru *)Pca9685Malloc(sizeof(ServoObjStru));
	if(ctx == NULL) {
		return NULL;
	}
	ctx->pca9685 = (Pca9685ObjStru *)pca9685;
	ctx->channel = channel;
	memcpy(&ctx->param, param, sizeof(Pca9685ParamStru));
	ServoSetPulse((ServoObjHdl)ctx, ctx->param.pwm);
	return (ServoObjHdl)ctx;
}

void ServoDeinit(ServoObjHdl hdl)
{
	ServoObjStru *ctx = (ServoObjStru *)hdl;
	if (ctx == NULL) {
		return;
	}
	Pca9685Free(ctx);
}

void ServoSetPulse(ServoObjHdl hdl, unsigned short pwm)
{
	ServoObjStru *ctx = (ServoObjStru *)hdl;

	if (pwm < ctx->param.minPwm || pwm > ctx->param.maxPwm) {
		log_err("err pwm %d", pwm);
		return;
	}
	if (pwm == ctx->param.pwm) {
		return;
	}

	ctx->param.pwm = pwm;
	log_dbg("set pwm %d", ctx->param.pwm);
	Pca9685SetPwm(ctx->pca9685, ctx->channel, ctx->param.deadZone, ctx->param.deadZone + ctx->param.pwm);
}

void ServoIncreasePwm(ServoObjHdl hdl)
{
	ServoObjStru *ctx = (ServoObjStru *)hdl;
	ServoSetPulse(hdl, ctx->param.pwm + ctx->param.step);
}

void ServoDecreasePwm(ServoObjHdl hdl)
{
	ServoObjStru *ctx = (ServoObjStru *)hdl;
	ServoSetPulse(hdl, ctx->param.pwm - ctx->param.step);
}

/*
 *  cycle: 50Hz = 20ms
 *	angle: [0,180]
 *
 *	angle = 0,   pwm = 0.5ms/20ms
 *	angle = 180, pwm = 2.5ms/20ms
 */
void ServoSetAngle(ServoObjHdl hdl, unsigned short angle)
{
	unsigned short tarPwm;

	if (angle > 180) {
		log_err("err angle %d", angle);
		return;
	}
	tarPwm = (int)((angle * (2000.0f / 180) + 500) * 4096 / 20000);
	ServoSetPulse(hdl, tarPwm);
}

DcMotorObjHdl DcMotorInit(Pca9685ObjHdl *pca9685, Pca9685ParamStru *param,
	unsigned short chanForward, unsigned short chanBackward)
{
	DcMotorObjStru *ctx;

	ctx = (DcMotorObjStru *)Pca9685Malloc(sizeof(DcMotorObjStru));
	if (ctx == NULL) {
		return NULL;
	}
	ctx->pca9685 = (Pca9685ObjStru *)pca9685;
	ctx->channelForward = chanForward;
	ctx->channelBackward = chanBackward;
	ctx->runState = DC_MOTOR_RUNSTATE_IDLE;
	memcpy(&ctx->param, param, sizeof(Pca9685ParamStru));
	return (DcMotorObjHdl)ctx;
}

void DcMotorDeinit(DcMotorObjHdl hdl)
{
	DcMotorObjStru *ctx = (DcMotorObjStru *)hdl;
	if (ctx == NULL) {
		return;
	}
	Pca9685Free(ctx);
}

void DcMotorRunPwm(DcMotorObjHdl hdl, int direction, unsigned short pwm)
{
	DcMotorObjStru *ctx = (DcMotorObjStru *)hdl;
	unsigned short workChan;

	if (pwm < ctx->param.minPwm || pwm > ctx->param.maxPwm) {
		log_err("err pwm %d", pwm);
		return;
	}

	if (direction == ctx->runState && pwm == ctx->param.pwm) {
		return;
	}

	if (ctx->runState != direction) {
		Pca9685SetPwmFull(ctx->pca9685, ctx->channelForward, false);
		Pca9685SetPwmFull(ctx->pca9685, ctx->channelBackward, false);
		ctx->runState = direction;
		log_dbg("runState change to %d", ctx->runState);
	}

	switch (direction) {
		case DC_MOTOR_RUNSTATE_FORWARD:
			workChan = ctx->channelForward;
			break;
		case DC_MOTOR_RUNSTATE_BACKWARD:
			workChan = ctx->channelBackward;
			break;
		default:
			return;
	}

	ctx->param.pwm = pwm;
	Pca9685SetPwm(ctx->pca9685, workChan, ctx->param.deadZone, ctx->param.deadZone + ctx->param.pwm);
}

void DcMotorFullSpeed(DcMotorObjHdl hdl, int direction)
{
	DcMotorObjStru *ctx = (DcMotorObjStru *)hdl;
	unsigned short workChan;

	switch (direction) {
		case DC_MOTOR_RUNSTATE_FORWARD:
			workChan = ctx->channelForward;
			break;
		case DC_MOTOR_RUNSTATE_BACKWARD:
			workChan = ctx->channelBackward;
			break;
		default:
			return;
	}

	if (ctx->runState != direction) {
		Pca9685SetPwmFull(ctx->pca9685, ctx->channelForward, false);
		Pca9685SetPwmFull(ctx->pca9685, ctx->channelBackward, false);
		ctx->runState = direction;
		log_dbg("runState change to %d", ctx->runState);
	}

	Pca9685SetPwmFull(ctx->pca9685, workChan, true);
}

/* End of file */
